#include "BasicIcpMatcher.h"

using namespace std;

BasicIcpMatcher::BasicIcpMatcher()
{
	name = "BasicIcpMatcher";
	printf("new BasicIcpMatcher\n");
	nr_iters = 30;
	correspondenceDistance = 0.07;
	rejectionThreshold = 0.5;
	//exit(0);
}

BasicIcpMatcher::~BasicIcpMatcher()
{
	printf("delete BasicIcpMatcher\n");
}

void BasicIcpMatcher::update(){
	icp.setMaximumIterations(nr_iters);
	icp.setMaxCorrespondenceDistance (correspondenceDistance); 
	//icp.setTransformationEpsilon (1e-8); 
	//icp.setEuclideanFitnessEpsilon (1); 
	icp.setRANSACOutlierRejectionThreshold(rejectionThreshold);
}

Transformation * BasicIcpMatcher::getTransformation(FrameNode * src, FrameNode * dst)
{
	update();
	printf("BasicIcpMatcher::getTransformation(%i,%i)\n",src->frame->id_number,dst->frame->id_number);
//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_to 	= dst->frame->keypoint_cloud;
//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_from 	= src->frame->keypoint_cloud;

	icp.setInputCloud (src->frame->keypoint_cloud);
	icp.setInputTarget(dst->frame->keypoint_cloud);
	pcl::PointCloud<pcl::PointXYZRGB> Final;

	icp.align(Final);
	
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->transformationMatrix = icp.getFinalTransformation();
	transformation->weight = 100;
	transformation->level = 1;
	transformation->next = 0;
	transformation->prev = 0;
	return transformation;
}
